# Find Rects Example
#
# 这个例子展示了如何使用april标签代码中的四元检测代码在图像中找到矩形。 四元检测算法以非常稳健的方式检测矩形，并且比基于Hough变换的方法好得多。 例如，即使镜头失真导致这些矩形看起来弯曲，它仍然可以检测到矩形。 圆角矩形是没有问题的！
# (但是，这个代码也会检测小半径的圆)...

import sensor, image, time
from machine import UART
from machine import LED

sensor.reset()
sensor.set_pixformat(sensor.RGB565) # 灰度更快(160x120 max on OpenMV-M7)
sensor.set_framesize(sensor.QQVGA)
sensor.set_auto_exposure(False) # 关闭自动曝光
sensor.set_auto_gain(False)     # 关闭自动增益
#sensor.set_vflip (True)
#sensor.set_hmirror (True)
sensor.skip_frames(time = 1000)

uart = UART(3, 9600)              # UART(3)P4-TX P5-RX
uart.init(9600,bits=8,parity=None,stop=1)
clock = time.clock()
led = LED("LED_BLUE")
led.on()
#******************************************************
define_find_one_point = 1   # 开关，用来设置是否只查找一个光点就确认红外激光点，为0则根据周围是否存在第二个光点在确定是激光点
red_threshold = [(76, 100, -10, 127, -18, 0)]       # 红点LAB阈值,(62, 100, 24, 52, -8, 7)
green_threshold = [(90, 100, -10, 10, -10, 6)]      # 绿点LAB阈值
red_point = [0,0]           # 存储红点坐标
#******************************************************

# 查找中心方框
rect = 0
def find_center_rect(img):
    global rect
    megnitude_max = 0
    for i in img:
        rect_center_x = i.w() /2 + i.x()
        rect_center_y = i.h() /2 + i.y()
        # 计算矩形中心点与图像中心点的距离
        distance_to_center = (abs(160//2 - rect_center_x)**2 + abs(120//2 - rect_center_y)**2)**0.5
        # 计算矩形左上角点与图像中心点的距离
        distance_to_point = (abs(i.x() - rect_center_x)**2 + abs(i.y() - rect_center_y)**2)**0.5
        if distance_to_center < 40 and i.magnitude() > megnitude_max and distance_to_point < 60:
            megnitude_max = i.magnitude()
            rect = i
    return rect

# 根据LAB查找光点
def find_dots_lab(rect,red):
    area = 0
    point_ = [0,0]
    for r in red:
        if r.area() > area:
            area = r.area()
            point_[0] = r.cx()
            point_[1] = r.cy()
    # 没有找到光点在继续从方框中逐个像素点扫描（用于扫描被黑线吃掉的光点）
    if area == 0 and rect != 0:
        for x in range(rect.x(), rect.x()+rect.w()):
            for y in range(rect.y(), rect.y()+rect.h()):
                if img.get_pixel(x,y)[0] > 240:
                    point_[0] = x
                    point_[1] = y
    return point_

# 根据RGB值查找光点（适合光点小的）
dots_rect = 0
def find_dots_rgb(img,rect):
    global dots_rect
    max_light = 0
    if rect != 0:
        dots_rect = rect
    expand = 15      # 设置方框周围往外延生多少查找光点
    if dots_rect != 0:
        for i in range(dots_rect.x()-expand,dots_rect.x()+dots_rect.w()+expand):
            for j in range(dots_rect.y()-expand,dots_rect.y()+dots_rect.h()+expand):
                rgbtemp = img.get_pixel(i,j)
                if isinstance(rgbtemp,tuple):
                    rgb = rgbtemp[0] + rgbtemp[1] + rgbtemp[2]
                    if rgb > 240*3:
                        if define_find_one_point == 1:
                            max_light=rgb
                            red_point[0]=i
                            red_point[1]=j
                            return red_point
                        else :
                            try:
                                rgbtemp1 = img.get_pixel(i+1,j)
                                rgb1 = rgbtemp1[0]+rgbtemp1[1]+rgbtemp1[2]
                                rgbtemp2 = img.get_pixel(i-1,j)
                                rgb2 = rgbtemp2[0]+rgbtemp2[1]+rgbtemp2[2]
                                rgbtemp3 = img.get_pixel(i,j+1)
                                rgb3 = rgbtemp3[0]+rgbtemp3[1]+rgbtemp3[2]
                                rgbtemp4 = img.get_pixel(i,j-1)
                                rgb4 = rgbtemp4[0]+rgbtemp4[1]+rgbtemp4[2]
                                if rgb1 > 240*3 or rgb2 > 240*3 or rgb3 > 240*3 or rgb4 > 240*3:
                                    max_light=rgb
                                    red_point[0]=i
                                    red_point[1]=j
                                    return red_point
                            except:
                                continue
    return red_point

# 发送坐标
def Send_coordinate(rect):
    coor = [(0,0),(0,0),(0,0),(0,0)]
    data = [0] * 13
    data[0]  = 0xa0
    data[1]  = 0xb0
    data[12] = 0xc0
    # 保存方框四个角坐标
    l = 9
    if rect != 0:
        for i in rect.corners():    # corners返回的坐标从左下角开始逆时针返回
            data[l]   = i[1]
            data[l-1] = i[0]
            l -= 2
    # 保存圆点中心坐标
    if red_point[0] != 0 and red_point[1] != 0:
        data[10] = red_point[0]
        data[11] = red_point[1]
    # 串口发送数据
    print(data)
    uart.write(bytearray(data))
    time.sleep(0.01)

# 画图
def draw(img,rect):
    if rect != 0:
        img.draw_rectangle(rect.rect(), color = (255, 0, 0))
        for p in rect.corners():
            img.draw_string (p[0], p[1], str(p[0])+','+str(p[1]), color=(255, 255, 255),mono_space=False)
    if red_point[0] != 0 and red_point[1] != 0:
        img.draw_circle(red_point[0], red_point[1], 3, color = (0, 255, 0))
        img.draw_string(red_point[0], red_point[1]+5, str(red_point[0])+','+str(red_point[1]), color=(255, 255, 255),mono_space=False)


while(True):
    clock.tick()
    img = sensor.snapshot()

    rect = find_center_rect(img.find_rects(threshold = 50000))      # 查找方框
    red_point = find_dots_lab(rect,img.find_blobs(red_threshold))   # 查找红光点

    Send_coordinate(rect)                                           # 发送四个点坐标给MCU
    draw(img,rect)                                                  # 画图

    #print("fps：" + str(clock.fps()))



